﻿using System;
using System.Drawing;
//using System.Windows;

namespace Agvlaser
{
    class RosHelper
    {
        /// <summary>
        /// 绘图区尺寸
        /// </summary>
        public static Size viewSize = new Size(0, 0);
        /// <summary>
        /// 原点坐标
        /// </summary>
        public static Point originPoint = new Point(0, 0);
        /// <summary>
        /// 地图有效区域
        /// </summary>
        public static Rectangle validArea = new Rectangle();
        /// <summary>
        /// 地图障碍物坐标集合
        /// </summary>
        public static Point[] mapObstaclePoints;
        /// <summary>
        /// 缩放比例
        /// </summary>
        public static float viewScale = 1f;

        /// <summary>
        /// 根据两点坐标计算角度
        /// </summary>
        /// <param name="pointStart">起点坐标</param>
        /// <param name="pointEnd">终点坐标</param>
        /// <returns></returns>
        public static double PointToAngle(System.Windows.Point pointStart, System.Windows.Point pointEnd)
        {
            //计算角度
            double angel;
            double offsetX = pointEnd.X - pointStart.X;
            double offsetY = pointEnd.Y - pointStart.Y;
            //90和270 属于特殊情况
            if (offsetX == 0)
            {
                if (offsetY > 0)
                    angel = 90;
                else
                    angel = 270;
            }
            else
            //正常情况
            {
                angel = Math.Atan(offsetY / offsetX) * 180 / Math.PI;
                if (offsetY >= 0)
                {
                    if (offsetX < 0)
                    {
                        if (angel <= 0)
                            angel += 180;
                    }
                }
                else
                if (offsetY < 0)
                {
                    if (angel > 0)
                        angel += 180;
                    else
                        angel += 360;
                }
            }
            return angel;
        }
        /// <summary>
        /// 根据两点坐标计算角度，Y轴反向
        /// </summary>
        /// <param name="pointStart">起点坐标</param>
        /// <param name="pointEnd">终点坐标</param>
        /// <param name="heigth">Y轴高度</param>
        /// <returns></returns>
        public static double PointToAngle(System.Windows.Point pointStart, System.Windows.Point pointEnd, double heigth)
        {
            //计算角度
            double angel;
            double offsetX = pointEnd.X - pointStart.X;
            double offsetY = (heigth - pointEnd.Y) - (heigth - pointStart.Y);
            //90和270 属于特殊情况
            if (offsetX == 0)
            {
                if (offsetY > 0)
                    angel = 90;
                else
                    angel = 270;
            }
            else
            //正常情况
            {
                angel = Math.Atan(offsetY / offsetX) * 180 / Math.PI;
                if (offsetY >= 0)
                {
                    if (offsetX < 0)
                    {
                        if (angel <= 0)
                            angel += 180;
                    }
                }
                else
                if (offsetY < 0)
                {
                    if (angel > 0)
                        angel += 180;
                    else
                        angel += 360;
                }
            }
            return angel;
        }

        /// <summary>
        /// 搜索有效图片区域
        /// </summary>
        /// <param name="map">原始地图</param>
        /// <param name="offset">公差，默认为向外保留两个像素，可以为负值</param>
        /// <returns></returns>
        public static Rectangle SearchEffectiveArea(RosSharp.RosBridgeClient.NavigationOccupancyGrid map, int offset = 2)
        {
            System.Drawing.Rectangle effectiveRectangle;
            //获取地图长宽
            int width = (int)map.info.width;
            int hight = (int)map.info.height;
            int wstart = 0;
            int wend = 0;
            int hstart = 0;
            int hend = 0;
            //搜索hstart
            for (int i = 0; i < hight; i++)
            {
                for (int j = 0; j < width; j++)
                {
                    if (map.data[i * width + j] != -1)
                    {
                        hstart = i;
                        break;
                    }
                }
                if (hstart != 0)
                    break;
            }
            //搜索hend
            for (int i = hight - 1; i >= 0; i--)
            {
                for (int j = 0; j < width; j++)
                {
                    if (map.data[i * width + j] != -1)
                    {
                        hend = i;
                        break;
                    }
                }
                if (hend != 0)
                    break;
            }
            //搜索wstart
            for (int i = 0; i < width; i++)
            {
                for (int j = 0; j < hight; j++)
                {
                    if (map.data[i + j * width] != -1)
                    {
                        wstart = i;
                        break;
                    }
                }
                if (wstart != 0)
                    break;
            }

            //搜索wend
            for (int i = width - 1; i >= 0; i--)
            {
                for (int j = 0; j < hight; j++)
                {
                    if (map.data[i + j * width] != -1)
                    {
                        wend = i;
                        break;
                    }
                }
                if (wend != 0)
                    break;
            }

            effectiveRectangle = new System.Drawing.Rectangle(wstart - offset, hstart - offset, (wend - wstart + 1) + offset, (hend - hstart + 1) + offset);
            return effectiveRectangle;
        }

        /// <summary>
        /// 坐标系位置到视图转换
        /// </summary>
        /// <param name="rectangle">原图片矩形坐标</param>
        /// <param name="ViewHight">视窗高度</param>
        /// <returns></returns>
        public static Rectangle AxisToView(Rectangle rectangle)
        {
            //界面的坐标系和真实坐标系Y轴相反
            rectangle.Y = viewSize.Height - 1 - rectangle.Y - rectangle.Height;
            //界面缩放比例
            rectangle.X = (int)(rectangle.X * viewScale);
            rectangle.Y = (int)(rectangle.Y * viewScale);
            rectangle.Width = (int)(rectangle.Width * viewScale);
            rectangle.Height = (int)(rectangle.Height * viewScale);
            //原点移动
            rectangle.X += originPoint.X;
            rectangle.Y += originPoint.Y;

            return rectangle;
        }
        /// <summary>
        /// 坐标系位置到视图转换
        /// </summary>
        /// <param name="point">坐标系中的坐标</param>
        /// <returns></returns>
        public static PointF AxisToView(PointF point)
        {
            //界面的坐标系和真实坐标系Y轴相反
            point.Y = viewSize.Height - 1 - point.Y;
            //界面缩放比例
            point.X = (int)(point.X * viewScale);
            point.Y = (int)(point.Y * viewScale);
            //原点移动
            point.X += originPoint.X;
            point.Y += originPoint.Y;
            return point;
        }
        /// <summary>
        /// 坐标系位置到视图转换
        /// </summary>
        /// <param name="x">真实系坐标 x</param>
        /// <param name="y">真实系坐标 y</param>
        /// <param name="width">真实系坐标 width</param>
        /// <param name="height">真实系坐标 height</param>
        /// <returns>界面坐标 Rectangle</returns>
        public static Rectangle AxisToView(int x, int y, int width, int height)
        {
            return AxisToView(new Rectangle(x, y, width, height));
        }
        /// <summary>
        /// 坐标系位置到视图转换
        /// </summary>
        /// <param name="x">真实系坐标 x</param>
        /// <param name="y">真实系坐标 y</param>
        /// <param name="width">真实系坐标 width</param>
        /// <param name="height">真实系坐标 height</param>
        /// <returns>界面坐标 Rectangle</returns>
        public static Rectangle AxisToView(float x, float y, float width, float height)
        {
            return AxisToView(new Rectangle((int)x, (int)y, (int)width, (int)height));
        }

        /// <summary>
        /// 界面的坐标系到真实坐标系转换
        /// </summary>
        /// <param name="point">界面坐标</param>
        /// <returns>真实坐标</returns>
        public static PointF ViewToAxis(System.Windows.Point point)
        {
            //原点移动
            point.X -= originPoint.X;
            point.Y -= originPoint.Y;
            //界面缩放比例
            point.X = (int)(point.X / viewScale);
            point.Y = (int)(point.Y / viewScale);
            //界面的坐标系和真实坐标系Y轴相反
            point.Y = viewSize.Height - 1 - point.Y;
            return new PointF((float)point.X, (float)point.Y);
        }
        /// <summary>
        /// 界面的坐标系到真实坐标系转换
        /// </summary>
        /// <param name="point"></param>
        /// <returns></returns>
        public static System.Windows.Point ViewToAxisPoint(System.Windows.Point point)
        {
            //原点移动
            point.X -= originPoint.X;
            point.Y -= originPoint.Y;
            //界面缩放比例
            point.X = (int)(point.X / viewScale);
            point.Y = (int)(point.Y / viewScale);
            //界面的坐标系和真实坐标系Y轴相反
            point.Y = viewSize.Height - 1 - point.Y;
            return point;
        }

        /// <summary>
        /// 世界坐标到坐标系转换
        /// </summary>
        /// <param name="x">世界坐标x，单位：米</param>
        /// <param name="y">世界坐标y，单位：米</param>
        /// <param name="map">地图</param>
        /// <returns></returns>
        public static System.Windows.Point WorldToAxis(float x, float y, RosSharp.RosBridgeClient.NavigationOccupancyGrid map)
        {
            //map在世界坐标的偏移
            float x0 = map.info.origin.position.x;
            float y0 = map.info.origin.position.y;
            //map的分辨率
            float resolution = map.info.resolution;
            //世界坐标在地图中的坐标
            float x1 = (x - x0) / resolution;
            float y1 = (y - y0) / resolution;
            return new System.Windows.Point(x1, y1);
        }
        /// <summary>
        /// 坐标系到世界坐标转换
        /// </summary>
        /// <param name="point">屏幕上的坐标</param>
        /// <returns></returns>
        public static PointF AxisToWorld(PointF point, RosSharp.RosBridgeClient.NavigationOccupancyGrid map)
        {
            float x, y;
            //map在世界坐标的偏移
            float x0 = map.info.origin.position.x;
            float y0 = map.info.origin.position.y;
            //map的分辨率
            float resolution = map.info.resolution;
            //世界坐标在地图中的坐标
            x = point.X * resolution + x0;
            y = point.Y * resolution + y0;
            return new PointF(x, y);
        }
        /// <summary>
        /// 世界坐标到视图坐标
        /// </summary>
        /// <param name="point"></param>
        /// <returns></returns>
        public static System.Windows.Point WorldToView(System.Windows.Point point)
        {
            point = WorldToAxis((float)point.X, (float)point.Y, GlobalVar.mapGrid);
            PointF pointF = AxisToView(new PointF((float)point.X, (float)point.Y));
            return new System.Windows.Point(pointF.X, pointF.Y);
        }

        /// <summary>
        /// 视图坐标到世界坐标
        /// </summary>
        /// <param name="point"></param>
        /// <returns></returns>
        public static PointF ViewToWorld(System.Windows.Point point)
        {
            //界面坐标系转真实坐标系
            PointF pointf = ViewToAxis(new System.Windows.Point(point.X, point.Y));
            //真实坐标系转世界坐标系
            pointf = AxisToWorld(pointf, GlobalVar.mapGrid);
            return pointf;
        }

        /// <summary>
        /// 获取地图中固定障碍坐标
        /// </summary>
        public static Point[] GetObstacleAxis(RosSharp.RosBridgeClient.NavigationOccupancyGrid map)
        {
            //障碍物坐标列表
            Point[] pointObstacle = new Point[validArea.Width * validArea.Height];
            //障碍物计数
            int numObstacle = 0;
            //绘制地图
            for (int h = validArea.Y; h < validArea.Y + validArea.Height; h++)
            {
                for (int w = validArea.X; w < validArea.X + validArea.Width; w++)
                {
                    int index = (int)map.info.width * h + w;
                    if ((index >= map.data.Length) || (index < 0))
                    {
                        continue;
                    }
                    else
                    {
                        //障碍物（墙壁）
                        if (map.data[index] == 100)
                        {
                            pointObstacle[numObstacle++] = new Point(w, h);
                        }
                    }
                }
            }
            Point[] point = new Point[numObstacle];
            for (int i = 0; i < numObstacle; i++)
            {
                point[i] = pointObstacle[i];
            }
            return point;
        }

        /// <summary>
        /// 四元素额Z值转角度值
        /// </summary>
        /// <param name="z"></param>
        /// <returns></returns>
        public static float QuaternionZToAngle(float z)
        {
            float angle = (float)(Math.Asin(z) * 2 * 180 / Math.PI);
            if (angle <= 0)
            {
                angle += 360;
            }
            return angle;
        }
    }
}
